#include"rclcpp/rclcpp.hpp"
#include"tf2_ros/transform_listener.h"
#include"tf2_ros/buffer.h"
#include"tf2/LinearMath/Quaternion.hpp"
#include"tf2_ros/transform_listener.h"
#include"geometry_msgs/msg/transform_stamped.hpp"
#include"tf2/utils.hpp"
#include"tf2_geometry_msgs/tf2_geometry_msgs.hpp"

#include<chrono>
#include<memory>

using namespace std;

class TFListener: public rclcpp::Node
{
private:
    rclcpp::TimerBase::SharedPtr timer;
    shared_ptr<tf2_ros::Buffer> buffer;
    shared_ptr<tf2_ros::TransformListener> listener;

    double y,p,r;

    void CallBackPrint()
    {
        try
        {
            const geometry_msgs::msg::TransformStamped transform = buffer->lookupTransform
                ("odom","base_footprint",rclcpp::Time(0),rclcpp::Duration::from_seconds(1.0f));
            const geometry_msgs::msg::Vector3& translation_result = transform.transform.translation;
            const geometry_msgs::msg::Quaternion& rotation_result = transform.transform.rotation;
            tf2::getEulerYPR(rotation_result,y,p,r);
            //Print
            RCLCPP_INFO(get_logger(),"平移偏量:(%.2f,%.2f,%.2f)"
                ,translation_result.x,translation_result.y,translation_result.z);
            RCLCPP_INFO(get_logger(),"欧拉角偏量(%.2f,%.2f,%.2f)",r,p,y);
        }
        catch(const exception& e)
        {
            RCLCPP_WARN(get_logger(),"无法获取TF变换,原因:%s",e.what());
        }
        return ;
    }

public:
    TFListener(const string& node_name):Node(node_name)
    {
        //init
        y = p = r = 0;

        buffer = std::make_shared<tf2_ros::Buffer>(this->get_clock());
        listener = std::make_shared<tf2_ros::TransformListener>(*this->buffer,this);
        timer = create_wall_timer(chrono::seconds(1),bind(&TFListener::CallBackPrint,this));
    }

    ~TFListener(){}
};

int main(int argc,char** argv)
{
    rclcpp::init(argc,argv);
    shared_ptr<TFListener> tf_node = make_shared<TFListener>("cpp_tf_node");
    rclcpp::spin(tf_node);
    rclcpp::shutdown();
    return 0;
}